// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcExtraSpin.h"

//---------------------------------------------------------------------------
MgcExtraSpin::MgcExtraSpin ()
    :
    m_kAxis(0.0,0.0,0.0),
    m_kQuat(0.0,0.0,0.0,0.0)
{
    m_fAngle = 0.0;
    m_iExtraSpins = 0;
    m_eInfo = NONE;
}
//---------------------------------------------------------------------------
void MgcExtraSpin::PreprocessKeys (int iNumKeys, MgcExtraSpin* akKey)
{
    // Determine interpolation type, compute extra spins, and adjust
    // angles accordingly.
    int i;
    for (i = 0; i < iNumKeys-1; i++)
    {
        MgcExtraSpin& rkKey0 = akKey[i];
        MgcExtraSpin& rkKey1 = akKey[i+1];
        MgcReal fDiff = rkKey1.m_fAngle - rkKey0.m_fAngle;
        if ( MgcMath::Abs(fDiff) < MgcMath::TWO_PI )
        {
            rkKey0.m_iExtraSpins = 0;
            rkKey0.m_eInfo = NONE;
        }
        else
        {
            rkKey0.m_iExtraSpins = int(fDiff/MgcMath::TWO_PI);
            
            if ( rkKey0.m_kAxis == rkKey1.m_kAxis )
                rkKey0.m_eInfo = SAME_AXIS;
            else if ( rkKey0.m_fAngle != 0.0 )
                rkKey0.m_eInfo = DIFF_AXIS_NO_ZERO;
            else
                rkKey0.m_eInfo = DIFF_AXIS_ZERO;
        }
    }

    // Eliminate any non-acute angles between successive quaternions.  This
    // is done to prevent potential discontinuities that are the result of
    // invalid intermediate value quaternions.
    for (i = 0; i < iNumKeys-1; i++)
    {
        MgcExtraSpin& rkKey0 = akKey[i];
        MgcExtraSpin& rkKey1 = akKey[i+1];
        if ( rkKey0.m_kQuat.Dot(rkKey1.m_kQuat) < 0.0 )
            rkKey1.m_kQuat = -rkKey1.m_kQuat;
    }

    // Clamp identity quaternions so that |w| <= 1 (avoids problems with
    // call to acos in SlerpExtraSpins).
    for (i = 0; i < iNumKeys; i++)
    {
        MgcExtraSpin& rkKey = akKey[i];
        if ( rkKey.m_kQuat.w < -1.0 )
            rkKey.m_kQuat.w = -1.0;
        else if ( rkKey.m_kQuat.w > 1.0 )
            rkKey.m_kQuat.w = 1.0;
    }
}
//---------------------------------------------------------------------------
void MgcExtraSpin::Interpolate (MgcReal fTime, const MgcExtraSpin& rkNextKey,
    MgcExtraSpin& rkInterpKey)
{
    // assert:  0 <= fTime <= 1

    switch ( m_eInfo )
    {
        case NONE:
        {
            rkInterpKey.m_kQuat = MgcQuaternion::Slerp(fTime,m_kQuat,
                rkNextKey.m_kQuat);
            break;
        }
        case SAME_AXIS:
        {
            rkInterpKey.m_fAngle = (1.0-fTime)*m_fAngle +
                fTime*rkNextKey.m_fAngle;
            rkInterpKey.m_kAxis = m_kAxis;
            rkInterpKey.m_kQuat.FromAngleAxis(rkInterpKey.m_fAngle,
                rkInterpKey.m_kAxis);
            break;
        }
        case DIFF_AXIS_NO_ZERO:
        {
            rkInterpKey.m_kQuat = MgcQuaternion::SlerpExtraSpins(fTime,
                m_kQuat,rkNextKey.m_kQuat,m_iExtraSpins);
            break;
        }
        case DIFF_AXIS_ZERO:
        {
            rkInterpKey.m_fAngle = (1.0-fTime)*m_fAngle +
                fTime*rkNextKey.m_fAngle;
            InterpolateAxis(fTime,m_kAxis,rkNextKey.m_kAxis,
                rkInterpKey.m_kAxis);
            rkInterpKey.m_kQuat.FromAngleAxis(rkInterpKey.m_fAngle,
                rkInterpKey.m_kAxis);
            break;
        }
    }
}
//---------------------------------------------------------------------------
void MgcExtraSpin::InterpolateAxis (MgcReal fTime, const MgcVector3& rkAxis0,
    const MgcVector3& rkAxis1, MgcVector3& rkInterpAxis)
{
    // assert:  rkAxis0 and rkAxis1 are unit length
    // assert:  rkAxis0.Dot(rkAxis1) >= 0
    // assert:  0 <= fTime <= 1

    MgcReal fCos = rkAxis0.Dot(rkAxis1);  // >= 0 by assertion
    if ( fCos > 1.0 ) // round-off error might create problems in acos call
        fCos = 1.0;

    MgcReal fAngle = MgcMath::ACos(fCos);
    MgcReal fInvSin = 1.0/MgcMath::Sin(fAngle);
    MgcReal fTimeAngle = fTime*fAngle;
    MgcReal fCoeff0 = MgcMath::Sin(fAngle - fTimeAngle)*fInvSin;
    MgcReal fCoeff1 = MgcMath::Sin(fTimeAngle)*fInvSin;

    rkInterpAxis = fCoeff0*rkAxis0 + fCoeff1*rkAxis1;
}
//---------------------------------------------------------------------------
